#初始化串口,波特率设置为115200
from machine import UART
uart = UART(3, baudrate=115200)

#摄像头初始化
import sensor,image
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_auto_gain(False)  # must be turned off for color tracking
sensor.set_auto_whitebal(False)  # must be turned off for color tracking
sensor.set_auto_exposure(True)
#19-125，8-350,2-125，9-275
sensor.skip_frames(time = 2000)

#检测的色块的坐标:X,Y,x向长度，y向长度
area = [145,115,45,45]
RGB = ['R','G','B','O','K']

#判断目标区域颜色
def get_color():
    img=sensor.snapshot()
    rgb=[0,0,0]
    for i in range(area[0],area[0]+area[2],2):
        for j in range(area[1],area[1]+area[3],2):
             rgb_img = img.get_pixel(i,j)
             rgb[0]+=rgb_img[0]
             rgb[1]+=rgb_img[1]
             rgb[2]+=rgb_img[2]
    img.to_grayscale()
    gray = 0.0
    for i in range(area[0],area[0]+area[2],2):
        for j in range(area[1],area[1]+area[3],2):
             gray += img.get_pixel(i,j)
    gray /= 529.0
    if 20<gray and gray<150:
        #判断颜色并发送
        if rgb[0]>rgb[1] and rgb[0]>rgb[2] and rgb[0]-min(rgb[1],rgb[2])>30000:
            # 红色
            uart.write('1')
            # print(RGB[0])
        elif rgb[1]>rgb[0] and rgb[1]>rgb[2] and rgb[1]-min(rgb[0],rgb[2])>20000:
            # 绿色
            uart.write('2')
            # print(RGB[1])
        elif rgb[2]>rgb[1] and rgb[2]>rgb[0] and rgb[2]-min(rgb[1],rgb[0])>20000:
            # 蓝色
            uart.write('3')
            # print(RGB[2])
        else:
            # 其他
            uart.write(RGB[3])
            # print(RGB[3]) #Other
    else:
        uart.write(RGB[4])
        # print(RGB[4]) #黑乎乎的
#    print('R')
#    print(rgb[0])
#    print('G')
#    print(rgb[1])
#    print('B')
#    print(rgb[2])
#    print(max(rgb[0],rgb[1],rgb[2])-min(rgb[0],rgb[1],rgb[2]))
#    print(gray)
#    print('\r\n')
    img.draw_rectangle(area[0:4],color = (255,255,255),thickness = 1,fill = False)


#识别圆环
XY=['X','Y']
def find_circle_centre():
    #识别圆环粗定位
    circle = [0,0,0,0]
    while circle[3] < 1:
        img = sensor.snapshot().crop(roi=[80,40,160,160]).histeq(adaptive=True, clip_limit=8)
        for c in img.find_circles(threshold = 4000, x_margin = 50, y_margin = 50, r_margin = 50,r_min = 24, r_max = 40, r_step = 5):
            circle[0] += c.x()
            circle[1] += c.y()
            circle[2] += c.r()
            circle[3] += 1
            #img.draw_circle(c.x(), c.y(), c.r(),color = (255,0,0))
    circle[0] = int(round(circle[0]/circle[3],0))
    circle[1] = int(round(circle[1]/circle[3],0))
    circle[2] = int(round(circle[2]/circle[3],0))
    #识别圆环精定位
    myroi = [circle[0]-35,circle[1]-35,70,70]
    circle_sum = [0,0,0,0]
    read = 0
    while circle_sum[3] < 4:
        read+=1
        if read > 4 :break
        img = sensor.snapshot().crop(roi=[80,40,160,160]).crop(roi=myroi).histeq(adaptive=True, clip_limit=8).midpoint(2, bias=0.1)
        for c in img.find_circles(threshold = 4000,r_min = 29, r_max = 33, r_step = 5):
            circle_sum[0] += c.x()
            circle_sum[1] += c.y()
            circle_sum[2] += c.r()
            circle_sum[3] += 1
            img.draw_circle(c.x(), c.y(), c.r(),color = (255,0,0))
    if circle_sum[3] == 0 : return
    X = int(round(circle_sum[0]/circle_sum[3],0))+circle[0]-35
    Y = int(round(circle_sum[1]/circle_sum[3],0))+circle[1]-35
    R = int(round(circle_sum[2]/circle_sum[3],0))
    #发送数据
    #圆中心的X坐标
    uart.write(XY[0])
    #圆中心与图像中心的X坐标差值
    uart.write(str(X-80))
    #圆中心的Y坐标
    uart.write(XY[1])
    #圆中心与图像中心的Y坐标差值
    uart.write(str(80-Y))
    """ print('X')
    print(X-80)
    print('Y')
    print(80-Y)
    print('R')
    print(R)
    print('\r\n') """


# 堆垛时识别物料
def find_material():
    circle = [0,0,0,0]
    while circle[3] < 5:
        img = sensor.snapshot().crop(roi=[80,40,160,160]).histeq(adaptive=True, clip_limit=8).laplacian(1, sharpen=True)
        for c in img.find_circles(threshold = 4000, x_margin = 50, y_margin = 50, r_margin = 50,r_min = 28, r_max = 32, r_step = 5):
            circle[0] += c.x()
            circle[1] += c.y()
            circle[2] += c.r()
            circle[3] += 1
            img.draw_circle(c.x(), c.y(), c.r(),color = (255,0,0))
    circle[0] = int(round(circle[0]/circle[3],0))
    circle[1] = int(round(circle[1]/circle[3],0))
    circle[2] = int(round(circle[2]/circle[3],0))
    uart.write(XY[0])
    uart.write(str(circle[0]-80))
    uart.write(XY[1])
    uart.write(str(80-circle[1]))
    """ print('X')
    print(circle[0])
    print('Y')
    print(circle[1])
    print('R')
    print(circle[2])
    print('\r\n') """


#指令标签
command = 'c'

#进入主循环
while True:
    if command is 'a':
        img = sensor.snapshot().crop(roi=[80,40,160,160]).histeq(adaptive=True, clip_limit=8)
        img.draw_circle(80,80,5,(255,0,0))
        img.draw_circle(80,80,15,(255,0,0))
        img.draw_circle(80,80,40,(255,0,0))
        img.draw_circle(80,80,60,(255,0,0))

    if command is 'b':
        get_color()

    if command is 'c':
        find_circle_centre()

    if command is 'd':
        find_material()
